#include "matrix4x4.h"
#include <cstring>
#include <iostream>
#include <math.h>
#include <sstream>

const float Matrix4x4::identity[Matrix4x4::VSIZE]= {1,0,0,0, 0,1,0,0, 0,0,1,0,  0,0,0,1};

Matrix4x4::Matrix4x4(){
    memcpy(this->m,identity,sizeof(m));
}

Matrix4x4::~Matrix4x4(){}

Matrix4x4::Matrix4x4(float theta,Point axis){
	if(axis.x1==1.0f){
		float rotationMatrix [VSIZE] ={1,0,0,0,  0,cos(theta),sin(theta),0,  0,-sin(theta),cos(theta),0,  0,0,0,1};
		memcpy(m,rotationMatrix,sizeof(rotationMatrix));
	}else if(axis.x2==1.0f){
		float rotationMatrix [VSIZE] ={cos(theta),0,sin(theta),0, 0,1,0,0,  -sin(theta),0,cos(theta),0,  0,0,0,1};
		memcpy(m,rotationMatrix,sizeof(rotationMatrix));
	}else if(axis.x3==1.0f){
		float rotationMatrix [VSIZE] ={cos(theta),sin(theta),0,0,  -sin(theta),cos(theta),0,0, 0,0,1,0,  0,0,0,1};
		memcpy(m,rotationMatrix,sizeof(rotationMatrix));
	}
}

Matrix4x4::Matrix4x4(float dx,float dy,float dz){
	float translationMatrix [VSIZE] ={1,0,0,0  ,0,1,0,0, 0,0,1,0, dx,dy,dz,1};
	memcpy(m,translationMatrix,sizeof(translationMatrix));
}

Matrix4x4::Matrix4x4(float ex,float ey,float ez,bool n){
	float escaleMatrix [VSIZE] = {ex,0,0,0, 0,ey,0,0,  0,0,ez,0,  0,0,0,1};
	memcpy(m,escaleMatrix,sizeof(escaleMatrix));
}

void Matrix4x4::loadIdentity(){
	memcpy(this->m,identity,sizeof(m));
}

Matrix4x4& Matrix4x4::operator*=(Matrix4x4& matrix){
	float aux=0;
	float maux[MATRIX_SIZE][MATRIX_SIZE];
	for (int k=0;k<MATRIX_SIZE;k++){
		for (int i=0;i<MATRIX_SIZE;i++){
			for(int j=0;j<MATRIX_SIZE;j++){
				aux+=matrix.m[j][i]*this->m[k][j];
			}
			maux[k][i] = aux;
			aux=0;
		}
	}
	memcpy(this->m,maux,sizeof(maux));
	return *this;
}

Point Matrix4x4::operator*(const Point& point)const{
    Point ans;
    float aux=0;
    for (int j=0;j<MATRIX_SIZE;j++){
        for (int i=0;i<MATRIX_SIZE;i++){
            aux+=this->m[i][j]*point[i+1];
        }
        ans[j+1]=aux;
		aux=0;
    }
    return ans;
}
